
package ticks.chromedome;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.InternalButton;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import ticks.chromedome.commands.ElevatorDown;
import ticks.chromedome.commands.ElevatorUp;
import ticks.chromedome.commands.PottyChairUp;
import ticks.chromedome.commands.ReleaseStopper;
import ticks.chromedome.commands.SetShooterSetpoint;
import ticks.chromedome.commands.SetSlashSetpoint;
import ticks.chromedome.commands.SetVisionEnabledCommand;
import ticks.chromedome.commands.ShooterSpeedControl;
import ticks.chromedome.commands.SlashJogDown;
import ticks.chromedome.commands.SlashJogUp;
import ticks.chromedome.commands.SuspensionFront;
import ticks.chromedome.commands.SuspensionRear;
import ticks.chromedome.commands.TurretJogLeft;
import ticks.chromedome.commands.TurretJogRight;
import ticks.chromedome.subsystems.Suspension;



/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
    Joystick driveJoystick;
    Joystick buttonboard;
    JoystickButton slash_to_top;
    JoystickButton slash_to_bridge;
    JoystickButton slash_to_bottom;
    JoystickButton slash_jog_up;
    JoystickButton slash_jog_down;
    JoystickButton elevator_up;
    JoystickButton elevator_down;
    JoystickButton release_balls;
    JoystickButton potty_chair;
    JoystickButton shooter_manual_mode;
    JoystickButton shooter_speed_bridge;
    JoystickButton shooter_speed_fender;
    JoystickButton turret_jog_left;
    JoystickButton turret_jog_right;
    JoystickButton auto1;
    JoystickButton auto2;
    JoystickButton auto3;
    JoystickButton vision_enabled;
    JoystickButton front_retract;
    JoystickButton front_extend;
    JoystickButton rear_retract;
    JoystickButton rear_extend;
    InternalButton debug;
    boolean[] auto_array;
    
    
    
    public OI(){
        driveJoystick = new Joystick(1);
        debug = new InternalButton();
        slash_to_top = new JoystickButton(driveJoystick, 1);
        slash_to_bridge = new JoystickButton(driveJoystick, 2);
        slash_to_bottom = new JoystickButton(driveJoystick, 3);
        slash_jog_up = new JoystickButton(driveJoystick, 3);
        slash_jog_down = new JoystickButton(driveJoystick, 4);
        elevator_up = new JoystickButton(driveJoystick, 5);
        elevator_down = new JoystickButton(driveJoystick, 6);
        release_balls = new JoystickButton(driveJoystick, 7);
        potty_chair = new JoystickButton(driveJoystick, 8);
        shooter_speed_bridge = new JoystickButton(driveJoystick, 10);
        shooter_speed_fender = new JoystickButton(driveJoystick, 11);
        shooter_manual_mode = new JoystickButton(driveJoystick, 9);
        turret_jog_left = new JoystickButton(driveJoystick, 12);
        turret_jog_right = new JoystickButton(driveJoystick, 13);
        vision_enabled = new JoystickButton(buttonboard, 1);
        front_retract = new JoystickButton(buttonboard, 2);
        front_extend = new JoystickButton(buttonboard, 3);
        rear_retract = new JoystickButton(buttonboard, 4);
        rear_extend = new JoystickButton(buttonboard, 5);
        slash_to_bottom.whenPressed(new SetSlashSetpoint(SetSlashSetpoint.SLASH_DOWN_VOLTAGE));
        slash_to_top.whenPressed(new SetSlashSetpoint(SetSlashSetpoint.SLASH_UP_VOLTAGE));
        slash_to_bridge.whenPressed(new SetSlashSetpoint(SetSlashSetpoint.SLASH_BRIDGE_VOLTAGE)); 
        slash_jog_down.whileHeld(new SlashJogDown());
        slash_jog_up.whileHeld(new SlashJogUp());
        elevator_up.whileHeld(new ElevatorUp());
        elevator_down.whileHeld(new ElevatorDown());
        release_balls.whileHeld(new ReleaseStopper());
        potty_chair.whenPressed(new PottyChairUp(PottyChairUp.potty_chair_up_time));
        shooter_speed_bridge.whenPressed(new SetShooterSetpoint(ShooterSpeedControl.bridge_speed));
        shooter_speed_fender.whenPressed(new SetShooterSetpoint(ShooterSpeedControl.fender_speed));
        turret_jog_left.whileHeld(new TurretJogLeft());
        turret_jog_right.whileHeld(new TurretJogRight());
        vision_enabled.whileHeld(new SetVisionEnabledCommand());
        front_retract.whenPressed(new SuspensionFront(Suspension.retracted));
        front_extend.whenPressed(new SuspensionFront(Suspension.extended));
        rear_extend.whenPressed(new SuspensionRear(Suspension.extended));
        rear_retract.whenPressed(new SuspensionRear(Suspension.retracted));
        SmartDashboard.putData("Send Debug Data to attached Debugger", debug);
       
    }
    
    private static OI instance=null;
     public static OI getInstance() {
        if(instance == null){
            instance = new OI();   
        }
        return instance;
     }
     /**
     *
     */
    public Joystick getDriveStick(){
        return driveJoystick;
    }
    
    public boolean shooterManualMode(){
        return shooter_manual_mode.get();
    }
    public double oiShooterSpeed(){
        return DriverStation.getInstance().getAnalogIn(1);
        
    }
    public int autoMode(){
        auto_array[0] = auto1.get();
        auto_array[1] = auto2.get();
        auto_array[2] = auto3.get();
        return Functions.booleanArrayToInt(auto_array);
    }
    
   
    
        
        
    //// CREATING BUTTONS
    // One type of button is a joystick button which is any button on a joystick.
    // You create one by telling it which joystick it's on and which button
    // number it is.
    // Joystick stick = new Joystick(port);
    // Button button = new JoystickButton(stick, buttonNumber);
    
    // Another type of button you can create is a DigitalIOButton, which is
    // a button or switch hooked up to the cypress module. These are useful if
    // you want to build a customized operator interface.
    // Button button = new DigitalIOButton(1);
    
    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.
    
    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:
    
    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenPressed(new ExampleCommand());
    
    // Run the command while the button is being held down and interrupt it once
    // the button is released.
    // button.whileHeld(new ExampleCommand());
    
    // Start the command when the button is released  and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenReleased(new ExampleCommand());
}

